#include <cstdio>

#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include "epuckDriver/OdometryPublisherMsg.h"
#include "epuckDriver/IRDataPublisherMsg.h"
#include "epuckDriver/CameraDataMsg.h"

#include <unistd.h>

void OdometryCallBack(const
epuckDriver::OdometryPublisherMsg::ConstPtr& odom){

   ROS_INFO("Odometry"); 
   ROS_INFO("position[x,y,theta]: %lf %lf %lf\n",
           odom->pose2d.x, odom->pose2d.y, odom->pose2d.theta); 
   ROS_INFO("velocity[x,y,theta]: %lf %lf %lf\n",        
           odom->twist.linear.x, odom->twist.linear.y, odom->twist.angular.z); 
   ROS_INFO("-------------------------------------------");
}

int main(int argc,char **argv){

    ros::init(argc,argv,"testOdomAll");
    ros::NodeHandle n;

    ros::Subscriber robo1 =
n.subscribe<epuckDriver::OdometryPublisherMsg>("epuckOdometry1",1,OdometryCallBack);

    ros::Subscriber robo2 =
n.subscribe<epuckDriver::OdometryPublisherMsg>("epuckOdometry2",1,OdometryCallBack);

    ros::Subscriber robo3 =
n.subscribe<epuckDriver::OdometryPublisherMsg>("epuckOdometry3",1,OdometryCallBack);

    ros::Subscriber robo4 =
n.subscribe<epuckDriver::OdometryPublisherMsg>("epuckOdometry4",1,OdometryCallBack);

    ros::Subscriber robo5 =
n.subscribe<epuckDriver::OdometryPublisherMsg>("epuckOdometry5",1,OdometryCallBack);

    ros::Subscriber robo6 =
n.subscribe<epuckDriver::OdometryPublisherMsg>("epuckOdometry6",1,OdometryCallBack);

    while(true)        
        ros::spinOnce();
    
    return 0;
}
